/*
 * SPDX-FileCopyrightText: 2010-2022 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: CC0-1.0
 */

#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "driver/pulse_cnt.h"
#include "driver/gpio.h"
#include "esp_sleep.h"
#include "driver/gptimer.h"
#include "tb6612.h"
#include "i2c_config.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "i2c_mpu6050.h"

static const char *TAG = "zzlesps3";

#define EXAMPLE_PCNT_HIGH_LIMIT  6000   // 计数值不能大于该值 使用65535系统会奔溃
#define EXAMPLE_PCNT_LOW_LIMIT  -6000   // 计数值不能小于该值

#define MAX_DELTA   5000 // 为了防止速度突变，计算速度时，判断达到EXAMPLE_PCNT_HIGH_LIMIT的标志 

#define E1_GPIO_A 4
#define E1_GPIO_B 5

#define E2_GPIO_A 6
#define E2_GPIO_B 7

#define MYPI 3.14159

pcnt_unit_handle_t pcnt_unit = NULL;


/* 
 * @函数作用      定时器回调函数
 * @传入参数      timer 定时器句柄
 *               *edata 定时器回调函数的一个指针，指向一个包含定时器警报事件数据的结构体
 *               *user_data 用户自带的数据，将传递进来的 user_data 转化为队列句柄，以便在回调函数中使用
 * @函数返回      1 发送队列数据成功， 0 失败
*/
static bool IRAM_ATTR myTimer_Callback(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *user_data)
{
    BaseType_t high_task_awoken = pdFALSE;
   // 将传进来的队列保存
   QueueHandle_t queue = (QueueHandle_t)user_data;
   /*
    * 这里可以写执行的逻辑，但是不建议写，因为防止阻塞，可以写简单的逻辑，通知另一个任务执行逻辑
   */
   static int time = 0;
#if 0
   time %= 20;
   time++;
#else
   ESP_ERROR_CHECK(pcnt_unit_get_count(pcnt_unit, &time));
#endif
   // 从中断服务程序（ISR）中发送数据到队列
   xQueueSendFromISR(queue, &time, &high_task_awoken);
   
   return high_task_awoken == pdTRUE;
}


/* 
 * @函数作用      定时器初始化配置
 * @传入参数      resolution_hz 定时器的分辨率
 *               alarm_count 触发警报时间的目标计数值 （达到定时（警报）计数值时 会去调用回调函数）
 * @函数返回      创建定时器回调队列
*/
QueueHandle_t myTimer_Init(uint32_t resolution_hz, uint64_t alarm_count)
{
    // 定义一个通用定时器句柄
    gptimer_handle_t gptimer_handle = NULL;

    // 创建一个队列，使用了队列
    QueueHandle_t queue = xQueueCreate(10, sizeof(int));

    if (!queue)
    {
       ESP_LOGE("queue", "creating queue failed");
       return NULL;
    }

    // 配置定时器
    gptimer_config_t timer_config = {
        .clk_src = GPTIMER_CLK_SRC_DEFAULT, // 定时器时钟来源 选择APB作为默认选项
        .direction = GPTIMER_COUNT_UP,      // 向上计数
        .resolution_hz = resolution_hz,     // 分辨率，即溢出频率 单位HZ 例如1e6=1MHz 1000000 hz
    };
    // 将配置设置到定时器实例中
    ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &gptimer_handle));

    // 配置警报
    gptimer_alarm_config_t gptimer_alarm_config = {
        .alarm_count = alarm_count,            // 设置报警时间 100000 us = 100ms ，到达100ms就会清零重新开始计数
        .flags.auto_reload_on_alarm = true,    // 开启循环报警模式 （重加载）
        .reload_count = 0,                     // 重装载值
    };
    // 将配置设置到定时器实例中
    ESP_ERROR_CHECK(gptimer_set_alarm_action(gptimer_handle, &gptimer_alarm_config));

    // 配置回调函数（警报函数）
    gptimer_event_callbacks_t ecallbacks = {
        .on_alarm = myTimer_Callback,
    };

    // 官方描述 : 确保gptimer_register_event_callbacks 这个函数不会试图阻塞，甚至是使用FreeRTOS的API 这里使用了队列
    ESP_ERROR_CHECK(gptimer_register_event_callbacks(gptimer_handle, &ecallbacks, queue));

    // 是能定时器
    ESP_ERROR_CHECK(gptimer_enable(gptimer_handle));

    // 开启定时器
    ESP_ERROR_CHECK(gptimer_start(gptimer_handle));

    return queue; // 返回配置好的 queue 句柄    
}


void i2c_mpu6050_task(void * arg)
{
#if 1
   int16_t accel[3], gyro[3];
   float pitch, roll, yaw;
   uint8_t flag_dmp = 1;
   
   ESP_LOGI(TAG, "i2c_mpu6050_task init1");

   // 配置I2C0-主机模式， 100K， SCL-12, SDA-11
   i2c_master_init(I2C_NUM_0, 400000, GPIO_NUM_12, GPIO_NUM_11);
   // 初始化mpu6050，检测mpu6050，并通过I2Cx总线写寄存器，对功能进行配置
   ESP_LOGI(TAG, "i2c_mpu6050_task init2");
   flag_dmp = mpu_dmp_init();

   ESP_LOGI(TAG, "i2c_mpu6050_task end");

   while(1) {
    // 读取 mpu6050的XYZ轴加速度计、XYZ轴陀螺仪数据
    i2c_mpu6050_get_accel_gyro(I2C_NUM_0, accel, gyro);
    ESP_LOGI(TAG, "Accel: x:%d\ty:%d\tz:%d\t||\tGyro: x:%d\ty:%d\tz:%d\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
    
    while(mpu_dmp_get_data(&pitch, &roll, &yaw) != 0);
    ESP_LOGI(TAG, "pitch:%f\troll:%f\tyaw:%f\t\n", pitch, roll, yaw);

    // vTaskDelay(pdMS_TO_TICKS(1000));
    vTaskDelay(1000 / portTICK_PERIOD_MS);
   }
#else
   int32_t count = 0;
   while(1) {
    count++;
    ESP_LOGI(TAG, "mycount:%ld\n", count);
    if (count > 100)
       count = 0;
    vTaskDelay(1000 / portTICK_PERIOD_MS);
   } 
#endif
}

void i2c_oled_task(void * arg)
{
   int16_t accel[3], gyro[3];
   float pitch, roll, yaw;
   uint8_t flag_dmp = 1;
   
   ESP_LOGI(TAG, "i2c_oled_task init1");

   // 配置I2C0-主机模式， 100K， SCL-12, SDA-11
   i2c_master_init(I2C_NUM_0, 400000, GPIO_NUM_12, GPIO_NUM_11);
   // 初始化mpu6050，检测mpu6050，并通过I2Cx总线写寄存器，对功能进行配置
   ESP_LOGI(TAG, "i2c_oled_task init2");
   flag_dmp = mpu_dmp_init();

   ESP_LOGI(TAG, "i2c_mpu6050_task end");

   while(1) {
    // 读取 mpu6050的XYZ轴加速度计、XYZ轴陀螺仪数据
    i2c_mpu6050_get_accel_gyro(I2C_NUM_0, accel, gyro);
    ESP_LOGI(TAG, "Accel: x:%d\ty:%d\tz:%d\t||\tGyro: x:%d\ty:%d\tz:%d\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
    
    while(mpu_dmp_get_data(&pitch, &roll, &yaw) != 0);
    ESP_LOGI(TAG, "pitch:%f\troll:%f\tyaw:%f\t\n", pitch, roll, yaw);

    // vTaskDelay(pdMS_TO_TICKS(1000));
    vTaskDelay(1000 / portTICK_PERIOD_MS);
   }
}

void app_main(void)
{
    ESP_LOGI(TAG, "config hardware timer start");
    int current_number = 0, prev_number = 0;
    QueueHandle_t queue = 0;
    
    // 初始化定时器 10毫秒进入回调函数一次读取编码器的数值
    queue = myTimer_Init(1000000, 10000);

    ESP_LOGI(TAG, "config hardware timer end");

    ESP_LOGI(TAG, "install pcnt unit start");
    // 配置esp32计数脉冲数的最大值的最小值
    pcnt_unit_config_t unit_config = {
        .high_limit = EXAMPLE_PCNT_HIGH_LIMIT,
        .low_limit = EXAMPLE_PCNT_LOW_LIMIT,
    };
#if 0
    pcnt_unit_handle_t pcnt_unit = NULL;
#endif
    ESP_ERROR_CHECK(pcnt_new_unit(&unit_config, &pcnt_unit));
    ESP_LOGI(TAG, "install pcnt unit end");

    ESP_LOGI(TAG, "set glitch filter start");
    pcnt_glitch_filter_config_t filter_config = {
        .max_glitch_ns = 1000,
    };
    ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit, &filter_config));
    ESP_LOGI(TAG, "set glitch filter end");

    ESP_LOGI(TAG, "install pcnt channels start");
    pcnt_chan_config_t chan_a_config = {
        .edge_gpio_num = E1_GPIO_A,
        .level_gpio_num = E1_GPIO_B,
    };
    pcnt_channel_handle_t pcnt_chan_a = NULL;
    ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_a_config, &pcnt_chan_a));

    pcnt_chan_config_t chan_b_config = {
        .edge_gpio_num = E1_GPIO_B,
        .level_gpio_num = E1_GPIO_A,
    };
    pcnt_channel_handle_t pcnt_chan_b = NULL;
    ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_b_config, &pcnt_chan_b));

    ESP_LOGI(TAG, "set edge and level actions for pcnt channels");
    ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE));
    ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
    ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE));
    ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));

    ESP_LOGI(TAG, "enable pcnt unit");
    ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit));
    ESP_LOGI(TAG, "clear pcnt unit");
    ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit));
    ESP_LOGI(TAG, "start pcnt unit");
    ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit));

#if CONFIG_EXAMPLE_WAKE_UP_LIGHT_SLEEP
    // EC11 channel output high level in normal state, so we set "low level" to wake up the chip
    ESP_ERROR_CHECK(gpio_wakeup_enable(E1_GPIO_A, GPIO_INTR_LOW_LEVEL));
    ESP_ERROR_CHECK(esp_sleep_enable_gpio_wakeup());
    ESP_ERROR_CHECK(esp_light_sleep_start());
#endif
    ESP_LOGI(TAG, "end pcnt unit");

    ESP_LOGI(TAG, "start tb6612 init");
    tb6612_init();
    ESP_LOGI(TAG, "end tb6612 init");
    seTduty(1000);

    float line_speed = 0;
    int delta_number = 0;

    // 创建 i2c_mpu6050_task 任务
    // xTaskCreate(i2c_mpu6050_task, "i2c_mpu6050_task", 8192, NULL, 3, NULL);

    // 创建 i2c_oled_task 任务
    // xTaskCreate(i2c_oled_task, "i2c_oled_task", 4096, NULL, 4, NULL); 

    while (1)
    {
#if 0
        ESP_ERROR_CHECK(pcnt_unit_get_count(pcnt_unit, &delta_number));
        ESP_LOGI(TAG, "Pulse count: %d", delta_number);
#else

        // 从队列中接收一个数据，不能在中断服务函数使用  队列为空时，阻塞超时的最大时间
        if (xQueueReceive(queue, &current_number, pdMS_TO_TICKS(2000)))
        {
           delta_number = current_number - prev_number;
           if (delta_number > MAX_DELTA ||  delta_number < -MAX_DELTA)
           {
               if (delta_number < -MAX_DELTA)
               {
                   delta_number = current_number + (EXAMPLE_PCNT_HIGH_LIMIT - prev_number);
               }
               else
               {
                   delta_number = current_number + (EXAMPLE_PCNT_LOW_LIMIT - prev_number);
               }
           }
           line_speed = delta_number * 0.048 * MYPI / (1024 * 0.01); // m/s 
           // rotate_speed = numer * 6000 / 1024; // r/min
        //    ESP_LOGI(TAG, "timer stopped, count=%d speed=%f m/s", current_number, line_speed);
           prev_number = current_number;
        } else {
           ESP_LOGW(TAG, "missed one count event");
        }

        ESP_LOGW(TAG, "*************************************************************************************************");

        vTaskDelay(pdMS_TO_TICKS(10));
#endif
    //   vTaskDelay(pdMS_TO_TICKS(100));
    }
}
